/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include "RemoteControlVehicle.h"

#define MaxPower 255
#define MinPower 128
#define MaxTurnPower 128

void RemoteControlVehicle::setup()
{
  receiver_.setup();
}

void RemoteControlVehicle::drive()
{
   receiver_.receive();

   if (receiver_.forwardPressState()==1)
    Serial.println("Up held this hard: ");

  if (receiver_.rightPressState()==1)
    Serial.println("Right held this hard: ");

  if (receiver_.leftPressState()==1)
    Serial.println("Left held this hard: ");

  if (receiver_.backwardPressState()==1)  
     Serial.println("Down held this hard: ");

  if (receiver_.accelPressState()==1) {  
     Serial.println("Circle just pressed");
  }

  if (receiver_.stopPressState()==1)   
     Serial.println("Square just pressed");

  if (receiver_.accelValue() > 0) {
     Serial.print("Accel value: ");
     Serial.println(receiver_.accelValue());
  }
  
   if (receiver_.stopPressState()==1) {
    driven_.stop();  
  } else {
    if (receiver_.forwardPressState()==1) {
      int power = receiver_.accelValue() / 100.0 * (MaxPower - MinPower) + MinPower;
      if (power > MaxPower)
         power = MaxPower;
      driven_.forward(power);
    } else if (receiver_.leftPressState()==1) {
      int power = MaxTurnPower + receiver_.angleValue() / 45.0 * (MaxTurnPower - MinPower);
      if (power < 0)
        power = MaxTurnPower - 16;
      if (power > MaxTurnPower)
        power = MaxTurnPower;
      driven_.turnLeft(power);
    } else if (receiver_.rightPressState()==1) {
      int power = MaxTurnPower - receiver_.angleValue() / 45.0 * (MaxTurnPower - MinPower);
        if (power < 0)
           power = MaxTurnPower - 16;
        if (power > MaxTurnPower)
          power = MaxTurnPower;
      driven_.turnRight(power);
    } else if (receiver_.backwardPressState()==1) {
      int power = receiver_.accelValue() / 100.0 * (MaxPower - MinPower) + MinPower;
      if (power > MaxPower)
         power = MaxPower;
      driven_.backward(power);
    }
  }
  
  delay(100);
}
